#include "controller.h"

static void f_Trapezoid_Intergral(PIDInstance *pid)
{
    pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
}

static void f_Changing_Integration_Rate(PIDInstance *pid)
{
    if (pid->Err * pid->Iout > 0)
    {
        // ���ֳ��ۻ�����
        // Integral still increasing
        if (abs(pid->Err) <= pid->CoefB)
            return; // Full integral
        if (abs(pid->Err) <= (pid->CoefA + pid->CoefB))
            pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA;
        else
            pid->ITerm = 0;
    }
}

static void f_Integral_Limit(PIDInstance *pid)
{
    static float temp_Output, temp_Iout;
    temp_Iout = pid->Iout + pid->ITerm;
    temp_Output = pid->Pout + pid->Iout + pid->Dout;
    if (abs(temp_Output) > pid->MaxOut)
    {
        if (pid->Err * pid->Iout > 0)
        {
            // ���ֳ��ۻ�����
            // Integral still increasing
            pid->ITerm = 0;
        }
    }

    if (temp_Iout > pid->IntegralLimit)
    {
        pid->ITerm = 0;
        pid->Iout = pid->IntegralLimit;
    }
    if (temp_Iout < -pid->IntegralLimit)
    {
        pid->ITerm = 0;
        pid->Iout = -pid->IntegralLimit;
    }
}

static void f_Derivative_On_Measurement(PIDInstance *pid)
{
		pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
}

static void f_Derivative_Filter(PIDInstance *pid)
{
    pid->Dout = pid->Dout * pid->dt / (pid->Derivative_LPF_RC + pid->dt) +
                pid->Last_Dout * pid->Derivative_LPF_RC / (pid->Derivative_LPF_RC + pid->dt);
}

static void f_Output_Filter(PIDInstance *pid)
{
    pid->Output = pid->Output * pid->dt / (pid->Output_LPF_RC + pid->dt) +
                  pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
}

static void f_Output_Limit(PIDInstance *pid)
{
    if (pid->Output > pid->MaxOut)
    {
        pid->Output = pid->MaxOut;
    }
    if (pid->Output < -(pid->MaxOut))
    {
        pid->Output = -(pid->MaxOut);
    }
}

static void f_Proportion_Limit(PIDInstance *pid)
{
    if (pid->Pout > pid->MaxOut)
    {
        pid->Pout = pid->MaxOut;
    }
    if (pid->Pout < -(pid->MaxOut))
    {
        pid->Pout = -(pid->MaxOut);
    }
}

// PID ERRORHandle Function
static void f_PID_ErrorHandle(PIDInstance *pid)
{
    /*Motor Blocked Handle*/
    if (pid->Output < pid->MaxOut * 0.001f || fabsf(pid->Ref) < 0.0001f)
        return;

    if ((fabsf(pid->Ref - pid->Measure) / fabsf(pid->Ref)) > 0.95f)
    {
        // Motor blocked counting
        pid->ERRORHandler.ERRORCount++;
    }
    else
    {
        pid->ERRORHandler.ERRORCount = 0;
    }

    if (pid->ERRORHandler.ERRORCount > 500)
    {
        // Motor blocked over 1000times
        pid->ERRORHandler.ERRORType = Motor_Blocked;
    }
}

void PID_Init(PIDInstance* pid, PID_Init_Config_s* _config)
{
    memset(pid, 0, sizeof(PIDInstance));
    memcpy(pid, _config, sizeof(PID_Init_Config_s));
    DWT_GetDeltaT(&pid -> DWT_CNT);
}

float PID_Calc(PIDInstance* pid, float ref, float measure)
{
    if(pid -> Improve & PID_ErrorHandle)
    {
        f_PID_ErrorHandle(pid);
    }

    pid -> dt = DWT_GetDeltaT(&pid -> DWT_CNT);

    pid -> Ref = ref;
    pid -> Measure = measure;
    pid -> Err = pid ->Ref - pid -> Measure;

    if(abs(pid -> Err) > pid -> DeadBand)
    {
        pid -> Pout = pid -> Kp * pid -> Err;
        pid -> ITerm = pid -> Ki * pid -> Err * pid -> dt;
        pid -> Dout = pid -> Kd * (pid -> Last_Err - pid -> Err) / pid -> dt;

        if(pid -> Improve & PID_Trapezoid_Intergral)
        {
            f_Trapezoid_Intergral(pid);
        }
        if(pid -> Improve & PID_Derivative_On_Measurement)
        {
            f_Derivative_On_Measurement(pid);
        }
        if(pid -> Improve & PID_ChangingIntegrationRate)
        {
            f_Changing_Integration_Rate(pid);
        }
        if(pid -> Improve & PID_Integral_Limit)
        {
            f_Integral_Limit(pid);
        }
        if(pid -> Improve & PID_DerivativeFilter)
        {
            f_Derivative_Filter(pid);
        }

        pid -> Iout += pid -> ITerm;

        pid -> Output = pid -> Pout + pid -> Iout + pid -> Dout;

        if(pid -> Improve & PID_OutputFilter)
        {
            f_Output_Filter(pid);
        }

        f_Output_Limit(pid);
    }
    else
    {
        pid -> Output = 0;
        pid -> ITerm = 0;
    }
    pid -> Last_Measure = pid -> Measure;
    pid -> Last_Err = pid -> Err;
    pid -> Last_ITerm = pid -> ITerm;
    pid -> Last_Dout = pid -> Dout;
    pid -> Last_Output = pid -> Output;
    
    return pid -> Output;
}
